

#include <pthread.h>
#include <system_error>
#include <unistd.h>
#include <memory>
#include <signal.h>
#include <fcntl.h>
#include <iostream>
#include <thread>
#include <mutex>
#include <vector>
#include <chrono>
#include <iomanip>
#include <sstream>


#define GPIO_EXPORT "/sys/class/gpio/export"
#define GPIO_UNEXPORT "/sys/class/gpio/unexport"
#define GPIO_DIRECTION "/sys/class/gpio/gpio%d/direction"
#define GPIO_VALUE "/sys/class/gpio/gpio%d/value"

// 注意：你需要根据实际的脉冲宽度和周期来调整下面的值
#define T_PW_SEC 0              // 脉冲宽度的秒部分
#define T_PW_NSEC 2000000       // 脉冲宽度的纳秒部分 (例如，1ms)
#define T_TT_SEC 0              // 总周期的秒部分
#define T_TT_NSEC 100000000     // 总周期的纳秒部分 (例如，100ms)

std::vector<std::chrono::steady_clock::time_point> gpioTimestamps;
std::mutex mtx;

void gpioCtlThread();
void captureThread();

// 用于格式化并打印时间戳的辅助函数
std::string formatTimestamp(const std::chrono::steady_clock::time_point& timePoint) {
    std::time_t now_c = std::chrono::system_clock::to_time_t(std::chrono::system_clock::now());
    auto milliseconds = std::chrono::duration_cast<std::chrono::milliseconds>(timePoint.time_since_epoch()) % 1000;

    std::stringstream ss;
    ss << std::put_time(std::localtime(&now_c), "%Y-%m-%d %H:%M:%S");
    ss << '.' << std::setfill('0') << std::setw(3) << milliseconds.count();

    return ss.str();
}


#include "./include/USBCam_API/USBCam_API.h"		//videoStreaming Control


static int32_t api_get_thread_policy (pthread_attr_t *attr)
{
    int32_t policy;

    int32_t rs = pthread_attr_getschedpolicy (attr, &policy);

    //assert (rs == 0);

    switch (policy)
    {
        case SCHED_FIFO:
            fprintf (stderr,"policy = SCHED_FIFO\n");
            break;
        case SCHED_RR:
            fprintf (stderr,"policy = SCHED_RR");
            break;
        case SCHED_OTHER:
            fprintf (stderr,"policy = SCHED_OTHER\n");
            break;
        default:
            fprintf (stderr,"policy = UNKNOWN\n");
            break;
    }


    return policy;
}



static void api_set_thread_policy (pthread_attr_t *attr)
{
    //int rs = pthread_attr_setschedpolicy (attr, policy);

    api_get_thread_policy (attr);

    struct sched_param param;

     pthread_attr_setschedpolicy(attr,SCHED_FIFO);
     //设置调度参数
     param.sched_priority = 99;

     pthread_attr_setschedparam(attr,&param);

     pthread_attr_setinheritsched(attr,PTHREAD_EXPLICIT_SCHED);

     pthread_detach(pthread_self());

}


void Set_Thread_attr()
{


    pthread_attr_t attr;
    int32_t     rs;
    rs = pthread_attr_init(&attr);

    if(rs == -1)
    {
        fprintf(stderr,"Error pthread_attr_init \r\n");
    }

    api_set_thread_policy (&attr);


}


void* USBCam_STREAM_DEAL(void*pUSBCam)
{
    Set_Thread_attr();

    Pix_Format Cfg;

    Cfg.u_PixFormat = 0;
    Cfg.u_Width = 1920;
    Cfg.u_Height = 1080;
    Cfg.u_Fps = 30;

    TST_USBCam_Video_DEAL_WITH         (pUSBCam,Cfg);

    /************************************************
    节点配置解除
    ************************************************/
    TST_USBCam_Video_DEAL_WITH_UNINIT  (pUSBCam);

    TST_USBCam_DELETE_DEVICE_POINT     (pUSBCam);

    return NULL;

}


void *GP3DStram = NULL;

bool EXIT = 1;

void SIG_QUIT(int signal)
{
    if(
       (signal == SIGINT) ||
       (signal == SIGQUIT)
       )
    {
        EXIT = 0;

        TST_USBCam_EVENT_LoopMode(GP3DStram,0);
    }

}



void captureThread()
{

    signal(SIGINT,SIG_QUIT);
    signal(SIGQUIT,SIG_QUIT);

    int32_t fd;
	
    /************************************************
    video stream control
    ************************************************/

    v4l2_dev_sys_data_t *pdevinfo   = NULL;


    int32_t ret =   TST_USBCam_DEVICE_FIND_ID     (&pdevinfo,0xFFFF,0xFFFF);

    if((ret == NULL_RETURN)||(ret == 0))
    {
		fprintf(stdout, "NULL Device\r\n");
		
        return;
    }

    fprintf(stdout,"device find result %d\r\n",ret);

    for(int32_t i = 0;i < ret;i++)
    {
        fprintf(stdout, "VID %04x\r\n", pdevinfo[i].Vid);

        fprintf(stdout, "PID %04x\r\n", pdevinfo[i].Pid);

        fprintf(stdout, "BCD %04x\r\n", pdevinfo[i].Bcd);

        fprintf(stdout, "ST0 %s\r\n",   pdevinfo[i].iManufacturer);

        fprintf(stdout, "ST1 %s\r\n",   pdevinfo[i].iProduct);

        fprintf(stdout, "ST2 %s\r\n",   pdevinfo[i].iSerialNumber);

        fprintf(stdout, "location %s\r\n",   pdevinfo[i].location);
    }


    void* pUSBCam =
    TST_USBCam_CREATE_DEVICE_POINT     (pdevinfo[0]);

    fd = open(pdevinfo[0].Device_Path, O_RDWR | O_NONBLOCK, 0);
	
	/************************************************
        在进行开启视频流之前，需打开设备，并将设备描述符传入，如过之前有打开过，将之前打开过的设备描述符传入即可
    ************************************************/

    int32_t returnVAL = TST_USBCam_Video_DEAL_WITH_INIT     (pUSBCam,fd);

    if(returnVAL != SUCCESS_RETURN)
    {
        fprintf(stderr,"TST_USBCam_Video_DEAL_WITH_INIT Fail ret %d\r\n",returnVAL);

        return;
    }

    pthread_t threadId;

    pthread_create(&threadId,   NULL,   USBCam_STREAM_DEAL,  pUSBCam);

    TST_USBCam_Video_STREAM_STATUS(pUSBCam,1);
	
	/************************************************
    在视频流开启后才能进行Processing Unit Control
    ************************************************/
    int32_t pu_val;

    TST_USBCam_PU_Get(pUSBCam,V4L2_CID_EXPOSURE_AUTO,&pu_val);

    fprintf(stderr,"V4L2_EXPOSURE_AUTO val:%d,%d\r\n",V4L2_CID_EXPOSURE_AUTO,pu_val);
	
	TST_USBCam_PU_Get(pUSBCam,V4L2_CID_BACKLIGHT_COMPENSATION,&pu_val);

    fprintf(stderr,"V4L2_CID_BACKLIGHT_COMPENSATION val:%d\r\n",pu_val);
	
	if(pu_val == 2){
		std::cout << "Camera enters triggle mode" << std::endl;
	}else{
		pu_val = 2;
		TST_USBCam_PU_Set(pUSBCam,V4L2_CID_BACKLIGHT_COMPENSATION,pu_val);
	}

    //TST_USBCam_PU_Set(pUSBCam,V4L2_CID_EXPOSURE_AUTO,3); //1:manual 3:AUTO

    if(pu_val == 1)
    {
        TST_USBCam_PU_Get(pUSBCam,V4L2_CID_EXPOSURE_ABSOLUTE,&pu_val);

        fprintf(stderr,"V4L2_CID_EXPOSURE_ABSOLUTE val:%d\r\n",pu_val);

        TST_USBCam_PU_Set(pUSBCam,V4L2_CID_EXPOSURE_ABSOLUTE,pu_val);
    }


    while(EXIT)
    {
        Frame_Buffer_Data*pFrame = TST_USBCam_GET_FRAME_BUFF(pUSBCam,0);

        if(pFrame != NULL)
        {
            char path[10];
#if 0
            if(pFrame->PixFormat.u_PixFormat == 0)
                snprintf(path,10,"%d.jpg",pFrame->index);
            else
                snprintf(path,10,"%d.bmp",pFrame->index);

            FILE    *File_fd = fopen(path,"w+");

            if(File_fd == NULL)
            {
                continue;
            }


            fwrite(pFrame->pMem,pFrame->buffer.bytesused,1,File_fd);


            fflush(File_fd);

            fclose(File_fd);
#endif
            if(pFrame->index >= 100)
            EXIT = 0;
		
			//获取当前时间
			auto now = std::chrono::steady_clock::now();
			//GPIO 设置高电平时间戳
			auto lastGpioTime = gpioTimestamps.back();
            auto diff = std::chrono::duration_cast<std::chrono::milliseconds>(now - lastGpioTime).count();
			//比较最近的一次设置高电平到当前捕获图像两时间差
            std::cout << "Difference with last GPIO timestamp: " << diff << " ms" << std::endl;
			
			
            fprintf(stderr,"pFrame->index:%02d PixFormat.u_Fps.:%d\r\n",pFrame->index,pFrame->PixFormat.u_Fps);
            TST_USBCam_SAVE_FRAME_RES(pUSBCam,pFrame);
        }
    }
	
	std::cout << "exit captureThread " << std::endl;
} 

void gpio_setup(int gpio, const char *direction) {
    char path[35];
    int fd;

    // 导出GPIO
    fd = open(GPIO_EXPORT, O_WRONLY);
    if (fd == -1) {
        perror("Export failed");
        exit(EXIT_FAILURE);
    }
    dprintf(fd, "%d", gpio);
    close(fd);

    // 设置GPIO方向
    snprintf(path, sizeof(path), GPIO_DIRECTION, gpio);
    fd = open(path, O_WRONLY);
    if (fd == -1) {
        perror("Direction setup failed");
        exit(EXIT_FAILURE);
    }
    write(fd, direction, strlen(direction));
    close(fd);
}

void gpio_write(int gpio, int value) {
    char path[35];
    int fd;

    snprintf(path, sizeof(path), GPIO_VALUE, gpio);
    fd = open(path, O_WRONLY);
    if (fd == -1) {
        perror("Write to GPIO failed");
        exit(EXIT_FAILURE);
    }
    dprintf(fd, "%d", value);
    close(fd);
}

void precise_sleep(long sec, long nsec) {
    struct timespec req, rem;

    req.tv_sec = sec;
    req.tv_nsec = nsec;
    while (nanosleep(&req, &rem) && errno == EINTR) {
        req = rem;
    }
}

void gpioCtlThread() {
    int gpio = 118; // GPIO编号，需要根据实际情况修改

    gpio_setup(gpio, "out");

    while (EXIT) {
        // 创建脉冲
        gpio_write(gpio, 1);
		
		auto now = std::chrono::steady_clock::now();
		std::lock_guard<std::mutex> lock(mtx);
		gpioTimestamps.push_back(now);
		
        precise_sleep(T_PW_SEC, T_PW_NSEC);
        gpio_write(gpio, 0);

        // 计算下一个脉冲前的延时
        precise_sleep(T_TT_SEC - T_PW_SEC, T_TT_NSEC - T_PW_NSEC);
    }
	std::cout << "exit gpioCtlThread " << std::endl;
}

int main()
{
	std::thread gpioThread(gpioCtlThread);
    std::thread videoThread(captureThread);
	
	gpioThread.join();
    videoThread.join();
	
	
	return 0;
}
